#include "FOC.h"
#include "FOC_DEF.h"

FOC::sMotor Motor_CH1, Motor_CH2;

void FOC::init(void){
    FOC::encoderInit();
    FOC::motorInit();
}

void FOC::Update(void){
    FOC::getEncoderPos();
    FOC::getMotorSpeed();
    FOC::CH1SpeedCtrl();
    FOC::CH2SpeedCtrl();
}

void FOC::LOG(void){
    if (Motor_CH1.state_encoder)
    {
        Serial.print("CH1 Pos info:\t");
        Serial.print(Motor_CH1.act_pos);
        Serial.print("\tCH1 Speed info:\t");
        Serial.print(Motor_CH1.act_speed);
    }
    else Serial.print("Encoder CH1 no connecting...");

    if (Motor_CH2.state_encoder)
    {
        Serial.print("\tCH2 Pos info:\t");
        Serial.print(Motor_CH2.act_pos);
        Serial.print("\tCH2 Speed info:\t");
        Serial.println(Motor_CH2.act_speed);
    }
    else Serial.println("\tEncoder CH2 no connecting...");

    // Serial.println(Motor_CH1.act_speed);
    
    
}